#include <SDL2/SDL.h>
#include <iostream>
#include <fcntl.h>
#include <unistd.h>
#include <cstdint>
#include <cstring>
#include <cstdlib>
#include <csignal>

// 全局变量
SDL_GameController *controller = nullptr;
volatile bool running = true; // 主循环控制

// 映射值到通道范围
uint16_t mapToChannel(int value, int minInput, int maxInput, int minOutput, int maxOutput) {
    return minOutput + (value - minInput) * (maxOutput - minOutput) / (maxInput - minInput);
}

// 初始化手柄
void initializeController() {
    if (SDL_Init(SDL_INIT_GAMECONTROLLER) < 0) {
        std::cerr << "Failed to initialize SDL: " << SDL_GetError() << std::endl;
        exit(1);
    }
    if (SDL_NumJoysticks() < 1) {
        std::cerr << "No joystick connected!" << std::endl;
        SDL_Quit();
        exit(1);
    }
    controller = SDL_GameControllerOpen(0);
    if (!controller) {
        std::cerr << "Failed to open controller: " << SDL_GetError() << std::endl;
        SDL_Quit();
        exit(1);
    }
    std::cout << "Controller initialized successfully." << std::endl;
}

// 读取手柄输入并映射到 RC 通道
void readControllerInput(uint16_t *channels) {
    SDL_GameControllerUpdate();
    int leftStickX = SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_LEFTX);
    int leftStickY = SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_LEFTY);
    int rightStickX = SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_RIGHTX);
    int rightStickY = SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_RIGHTY);

    int leftTrigger = SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_TRIGGERLEFT);
    int rightTrigger = SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_TRIGGERRIGHT);

    // 映射摇杆值到 RC 通道（1000 ~ 2000）
    channels[0] = mapToChannel(leftStickX, -32768, 32767, 1000, 2000);  // Yaw
    channels[1] = mapToChannel(leftStickY, -32768, 32767, 1000, 2000);  // Throttle
    channels[2] = mapToChannel(rightStickX, -32768, 32767, 1000, 2000); // Roll
    channels[3] = mapToChannel(rightStickY, -32768, 32767, 1000, 2000); // Pitch

    channels[4] = mapToChannel(leftTrigger, 0, 32767, 1000, 2000); 
    channels[5] = mapToChannel(rightTrigger, 0, 32767, 1000, 2000); 

    // 读取按钮输入并设置辅助通道
    channels[6] = SDL_GameControllerGetButton(controller, SDL_CONTROLLER_BUTTON_A) ? 2000 : 1000; // 解锁/锁定
    channels[7] = SDL_GameControllerGetButton(controller, SDL_CONTROLLER_BUTTON_B) ? 2000 : 1000; // 解锁/锁定
    channels[8] = SDL_GameControllerGetButton(controller, SDL_CONTROLLER_BUTTON_X) ? 2000 : 1000; // 解锁/锁定
    channels[9] = SDL_GameControllerGetButton(controller, SDL_CONTROLLER_BUTTON_Y) ? 2000 : 1000; // 解锁/锁定

    std::cout << "channels[0]: " << channels[0] << std::endl;
    std::cout << "channels[1]: " << channels[1] << std::endl;
    std::cout << "channels[2]: " << channels[2] << std::endl;
    std::cout << "channels[3]: " << channels[3] << std::endl;
    std::cout << "---------------------------------" << std::endl;
    std::cout << "channels[4]: " << channels[4] << std::endl;
    std::cout << "channels[5]: " << channels[5] << std::endl;
    std::cout << "---------------------------------" << std::endl;
    std::cout << "channels[6]: " << channels[6] << std::endl;
    std::cout << "channels[7]: " << channels[7] << std::endl;
    std::cout << "channels[8]: " << channels[8] << std::endl;
    std::cout << "channels[9]: " << channels[9] << std::endl;
    std::cout << "---------------------------------" << std::endl;
}

// 信号处理器，用于安全退出
void handleSignal(int signal) {
    running = false;
    std::cout << "Exiting..." << std::endl;
}

int main() {
    // 捕捉 Ctrl+C 信号
    signal(SIGINT, handleSignal);

    // 初始化手柄
    initializeController();

    std::cout << "Starting main loop. Press Ctrl+C to exit." << std::endl;

    uint16_t channels[16] = {0}; // RC 通道

    SDL_Event event;
    while (running) {
        // 读取手柄输入
        // readControllerInput(channels);

        while (SDL_PollEvent(&event)) {
            if (event.type == SDL_CONTROLLERAXISMOTION) {
                // 捕获摇杆输入
                std::cout << "Axis " << event.caxis.axis << " value: " << event.caxis.value << std::endl;
            } else if (event.type == SDL_CONTROLLERBUTTONDOWN) {
                // 捕获按键按下
                std::cout << "Button " << event.cbutton.button << " pressed" << std::endl;
            } else if (event.type == SDL_CONTROLLERBUTTONUP) {
                // 捕获按键释放
                std::cout << "Button " << event.cbutton.button << " released" << std::endl;
            }
        }
        SDL_Delay(16); // 降低 CPU 占用率
    }

    // 资源清理
    SDL_GameControllerClose(controller);
    SDL_Quit();

    return 0;
}
